/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
	\file template.h
	\brief header file for template

										\author Jack Elston
						 $Date: 2006/10/25 13:34:45 $
*/

#ifndef __CHAIN_H
#define __CHAIN_H

#include "threadedPipe.h"
#include "matrix.h"

// number of samples to use over an orbit
//   2*pi*r / Vt - where r = ORBIT_RADIUS + 10 for tracking error
#define MAX_NUM_RSSI 65  

class RSSIChain : public ThreadedPipe
{
	typedef enum { 
		STANDBY = 1, 
		INITIALIZING, 
		CHAINING
	} FCMode;

	typedef enum {
		MAXIMIN_CONTROLLER = 0,
		RR_CONTROLLER
	} ControllerMode;

	private:
		ClientList chained_vehicles;

		TimeStamp last_chaining_loop;
		TimeStamp last_orbit_ts;

		void chainTargets();

		FCMode fc_mode;
		bool send_goto_wp;
		ControllerMode controlMode;

		GPSLLA_t     orbit_point; // orbit point, in geodectic cordinates for autopilot
		CartVector_t control_point; // control point, in ECEF is converted to orbit_point
		CartVector_t Vcp;        // velocity of orbit point, in ECEF 

		int          useIND; 
		CartVector_t ecef[MAX_NUM_RSSI];

		struct RSSIClient_STR {
			String       name;
			LinkStatus_t link[MAX_NUM_RSSI];
			float        avgRSSI;
			CartVector_t rssiVEC; // gradient vector of rssi field
		} rssiClient[MAX_CLIENTS]; 

		CartVector_t  esVEC; // gradient vector of performance function

		int clientRSSI_ind[MAX_CLIENTS];
		int numClients;

		OrbitValues_t orbit;
		ChainGains_t  gains;
		double dT;
		double orbit_dT;

		void DoE_gradEst();
		void command_orbit_controller();
		void maximin_controller();
		void rr_controller();
		bool isValidControll;

	public:
		RSSIChain(String name);
		~RSSIChain() {}

		bool init(ShmStruct * shmStructPtr);
		void resetChain(); // reset controller to default state

		void update();
};

#endif
